#include <MsTimer2.h>

#define BRANCO    85
#define VERDE     180
#define VERMELHO  250
#define PRETO     970

#define DEBUG 0

#define Ta 50 // Tempo de amostragem (em milisegundos)
#define STEP_TIME 10 // Tempo de cada degrau (em segundos)
#define FRENTE 1
#define TRAS 0
#define PPR_A 330 // Pulses Per Revolution - Motor A
#define PPR_B 430 // Pulses Per Revolution - Motor B


int colorSensor = 0;    
int leitura = 0;       
int corDetectada = 0;

long int contador_A[2] = {0,0};
long int contador_B[2] = {0,0};
long double tempo = 0;
int index = 1;
float velocidade_A[3];
float velocidade_B[3];

static boolean flag = LOW;

int motor_A_speed = 0; // velocidade inicial 0 <= motor_A_speed <= 250  
int motor_B_speed = 0;
int sentido_rot;
int numero_voltas = 200;

//---------Conexao dos pinos MOTOR_A---------//
int b1_A = 7; //motor A bit1 
int b2_A = 8; //motor A bit1 
int EN_A = 9; // Enable motor A
// Encoder_motor_A -> digital 02

//---------Conexao dos pinos MOTOR_B---------//
int b1_B = 5; //motor B bit1 
int b2_B = 6; //motor B bit1 
int EN_B = 10; // Enable motor B
// Encoder_motor_B -> digital 03

void setup()
{
  Serial.begin(9600);

  //MsTimer2::set(Ta, Amostragem);

  pinMode(EN_A,OUTPUT);
  pinMode(b1_A,OUTPUT);
  pinMode(b2_A,OUTPUT);
  pinMode(EN_B,OUTPUT);
  pinMode(b1_B,OUTPUT);
  pinMode(b2_B,OUTPUT);


  pinMode(13, OUTPUT); // LED pin
  pinMode(2, INPUT); // interrupt 0 - Encoder_Motor_A
  pinMode(3, INPUT); // interrupt 1 - Encoder_Motor_B

  // Default = FRENTE
  digitalWrite(b1_A, HIGH);
  digitalWrite(b2_A, LOW);
  digitalWrite(b1_B, HIGH);
  digitalWrite(b2_B, LOW);
  
  sentido_rot = FRENTE;

//  attachInterrupt(0, conta_A, CHANGE); // Encoder_motor_A -> digital 02
//  attachInterrupt(1, conta_B, CHANGE); // Encoder_motor_B -> digital 03
  
  //MsTimer2::start(); 
  
  delay(100);
  
}

void Amostragem()
{
  tempo = tempo + Ta;

  velocidade_A[0] = ( (contador_A[1]-contador_A[0])*(2*PI/PPR_A))/Ta; // velocidade em rad/ms
  velocidade_B[0] = ( (contador_B[1]-contador_B[0])*(2*PI/PPR_B))/Ta; // velocidade em rad/ms
  
  //Serial.println( (int)(velocidade[0]*100000) );

  contador_A[0] = contador_A[1];
  contador_B[0] = contador_B[1];
}


void conta_A()
{

  contador_A[1] = contador_A[1]+1;

  if( DEBUG )
  {
    digitalWrite(13,flag);
    flag = !flag;
  }

}


void conta_B()
{

  contador_B[1] = contador_B[1]+1;

  if( DEBUG )
  {
    digitalWrite(13,flag);
    flag = !flag;
  }

}


void loop()
{
  
  for( int i = 0; i < 120; i += 2)
  {
      motor_A_speed += 2;
      motor_B_speed += 2; 
      analogWrite( EN_A, motor_A_speed );
      analogWrite( EN_B, motor_B_speed );
      delay(5);
  }

  while( corDetectada != VERMELHO )
  {
    leitura = analogRead(colorSensor);    // read the value from the sensor
       Serial.println(leitura);
  
    if( leitura <= (BRANCO + (VERDE - BRANCO)/2 ) )
    {
      //Serial.println("BRANCO!");
      corDetectada = BRANCO;
    }
    else 
    {
      if ( (leitura > (BRANCO + (VERDE - BRANCO)/2 )) && (leitura <= (VERDE + (VERMELHO - VERDE)/2 )) )
      {
            //Serial.println("VERDE!");
            corDetectada = VERDE;
      }
      else
      {
        if( (leitura > (VERDE + (VERMELHO - VERDE)/2 )) && (leitura <= (VERMELHO + 50 )) )
        {
             //Serial.println("VERMELHO!");
             corDetectada = VERMELHO;
        }
        else
        {
            //Serial.println("PRETOOO!");
            corDetectada = PRETO;
        }
      
      }
    
    }
    
    
  delay(20);  
  }

  //delay(2000);
  
  digitalWrite(b1_A, HIGH);
  digitalWrite(b2_A, HIGH);
  digitalWrite(b1_B, HIGH);
  digitalWrite(b2_B, HIGH); 
  motor_A_speed = 0;
  motor_B_speed = 0;
  analogWrite( EN_A, motor_A_speed );
  analogWrite( EN_B, motor_B_speed );
  
}
